package com.campos.process;


import android.location.Location;

import com.campos.common.*;

import com.campos.collection.IMUSensor;
import com.campos.common.ContextData;

import java.text.SimpleDateFormat;
import java.util.ArrayList;
import java.util.Date;

public class DataCalib {

    public void calibrate(){
        for(int i = 0; i< ContextData.instance.imageList.size(); i++) {
            ImageData imd =  ContextData.instance.imageList.get(i);
            Date time =imd.date;
            Location loc = imd.pos;
            IMUSensor.IMUSensorData mdata = getMeanImuData(time);
            imd.heading = mdata.angles[0];
            imd.pitch = mdata.angles[1];
            imd.roll = mdata.angles[2];
            //ContextData.instance.imuDataListCalib.add(mdata);
        }
    }

    public IMUSensor.IMUSensorData getMeanImuData(Date dt){
        double maxtime = dt.getTime() + 100;
        double mintime = dt.getTime() - 100;
        ArrayList<IMUSensor.IMUSensorData> templist = new ArrayList<>();
        for(int i = 0; i< ContextData.instance.imuDataList.size(); i++){
            IMUSensor.IMUSensorData sd  = ContextData.instance.imuDataList.get(i);
            double tt = sd.date.getTime();
            if(tt > mintime && tt < maxtime ){
                templist.add(sd);
            }
        }

        IMUSensor.IMUSensorData meanData = new IMUSensor.IMUSensorData();
        if(templist.size() <= 0) return meanData;
        for(int i=0;i< templist.size();i++){
            IMUSensor.IMUSensorData sd  = templist.get(i);
            meanData.angles[0] += sd.angles[0];
            meanData.angles[1] += sd.angles[1];
            meanData.angles[2] += sd.angles[2];

            meanData.gravity[0] += sd.gravity[0];
            meanData.gravity[1] += sd.gravity[1];
            meanData.gravity[2] += sd.gravity[2];
        }
        meanData.angles[0] /= templist.size();
        meanData.angles[1] /= templist.size();
        meanData.angles[2] /= templist.size();
        meanData.gravity[0] /= templist.size();
        meanData.gravity[1] /= templist.size();
        meanData.gravity[2] /= templist.size();
        return meanData;
    }
}
